/*
**Copyright (C) 2022, HITCRT_VISION, all rights reserved.
*/
#pragma once
#include <Eigen/Dense>
#include <boost/any.hpp>
#include <cmath>
#include <iostream>
#include <memory>
#include <opencv2/core/mat.hpp>
#include <stdexcept>

#include "opencv2/opencv.hpp"
#include "stdlib.h"

namespace hitcrt {
class KF {
   public:
    KF(const int measureDim, const int stateDim)
        : m_measureDimension(measureDim),
          m_stateDimension(stateDim),
          m_transitionMatrix(cv::Mat(stateDim, stateDim, CV_64F)),
          m_measurementMatrix(cv::Mat(measureDim, measureDim, CV_64F)),
          m_processNoiseCov(cv::Mat(stateDim, stateDim, CV_64F)),
          m_measurementNoiseCov(cv::Mat(measureDim, measureDim, CV_64F)),
          m_errorCovPrior(cv::Mat(stateDim, stateDim, CV_64F)),
          m_statePrior(cv::Mat(stateDim, 1, CV_64F)),
          m_statePost(cv::Mat::zeros(stateDim, 1, CV_64F)),
          m_measurement(cv::Mat::zeros(measureDim, 1, CV_64F)),
          m_errorCovPost(cv::Mat(stateDim, stateDim, CV_64F)),
          m_gain(cv::Mat(stateDim, measureDim, CV_64F)),
          m_residual(cv::Mat(measureDim, 1, CV_64F)),
          m_transformation(cv::Mat(measureDim, 1, CV_64F)) {};
    ~KF() {};
    virtual void init(const std::vector<boost::any> &param) = 0;
    virtual void setParam(const std::vector<boost::any> &param) = 0;
    virtual void predict();
    virtual void stateUpdate();
    virtual void measureUpdate();
    virtual const cv::Mat &getResult();
    cv::Mat m_transformation;  // 先验估计坐标转换（EKF修改用）

   protected:
    virtual void stateUpdataImpl();
    virtual void measureUpdateImpl();
    virtual void checkStateUpdate();
    virtual void checkMeasureUpdate();

    cv::Mat m_statePost;    // 状态矩阵
    cv::Mat m_measurement;  // 测量的传感器量 这里直接为坐标值
    cv::Mat m_transitionMatrix;  // 预测更新矩阵
    cv::Mat
        m_measurementMatrix;  // 即将预测的状态向量转换为传感器测量量的转换矩阵

    cv::Mat m_processNoiseCov;      // 过程噪声协方差矩阵 Q
    cv::Mat m_measurementNoiseCov;  // 测量噪声协方差矩阵

    cv::Mat m_errorCovPost;
    cv::Mat m_errorCovPrior;  // P
    cv::Mat m_statePrior;
    cv::Mat m_residual;  // 残差（EKF修改用）

    const int m_measureDimension;
    const int m_stateDimension;
    double m_deltaTime;

    // 输入参数异常处理类
    class paramNumberError : public std::exception {
       public:
        paramNumberError() = delete;
        explicit paramNumberError(const std::string &formula) {
            buildErrormessage(formula);
        }
        virtual const char *what() const noexcept override {
            return m_errorMessage.c_str();
        }

       private:
        void buildErrormessage(const std::string &formula) {
            m_errorMessage = formula;
            m_errorMessage +=
                ":The number of param is false!! Please input right param";
        }
        std::string m_errorMessage;
    };

   private:
    cv::Mat m_gain;  // 卡尔曼增益

    // 时间更新方程异常处理类
    class stateUpdateDimErr : public std::exception {
       public:
        stateUpdateDimErr() = delete;
        explicit stateUpdateDimErr(const std::string &formula) {
            buildErrormessage(formula);
        }
        virtual const char *what() const noexcept override {
            return m_errorMessage.c_str();
        }

       private:
        void buildErrormessage(const std::string &formula) {
            m_errorMessage = "StateUpdate():The ";
            m_errorMessage += formula;
            m_errorMessage +=
                " formula is false!! Matrix dimensions must agree!";
        }
        std::string m_errorMessage;
    };
    // 测量更新方程异常处理类
    class measUpdateDimErr : public std::exception {
       public:
        measUpdateDimErr() = delete;
        explicit measUpdateDimErr(const std::string &formula) {
            buildErrormessage(formula);
        }
        virtual const char *what() const noexcept override {
            return m_errorMessage.c_str();
        }

       private:
        void buildErrormessage(const std::string &formula) {
            m_errorMessage = "MeasUpdate():The ";
            m_errorMessage += formula;
            m_errorMessage +=
                " formula is false!! Matrix dimensions must agree!";
        }
        std::string m_errorMessage;
    };
};
}  // namespace hitcrt
